import yaml
import os

def load_yaml(file_path):
    """加载 YAML 文件"""
    try:
        with open(file_path, 'r') as file:
            data = yaml.safe_load(file)
        return data
    except FileNotFoundError:
        print("YAML 文件未找到！")
        return None
    except yaml.YAMLError as e:
        print(f"YAML 解析错误: {e}")
        return None

def save_yaml(file_path, data):
    """保存更新后的 YAML 文件"""
    try:
        with open(file_path, 'w') as file:
            yaml.safe_dump(data, file, default_flow_style=False, allow_unicode=True)
        print("YAML 文件已成功更新！")
    except Exception as e:
        print(f"保存文件时出错: {e}")

def update_parameters(parameters):
    """
    交互式更新 YAML 文件中的 `parameters` 字段
    :param parameters: YAML 文件中的 parameters 字典
    """
    print("当前参数：")
    for key, value in parameters.items():
        print(f"{key}: {value}")

    print("\n提示：输入新的值以更新参数，直接按回车跳过")
    for key in parameters.keys():
        new_value = input(f"请输入参数 '{key}' 的新值（当前值: {parameters[key]}）：").strip()
        if new_value:
            # 尝试将输入转换为合适的类型
            if new_value.isdigit():
                parameters[key] = int(new_value)
            elif new_value.replace('.', '', 1).isdigit():
                parameters[key] = float(new_value)
            else:
                parameters[key] = new_value
            print(f"参数 '{key}' 已更新为：{parameters[key]}")

    # 确保 namesspace 与 vehicle_id 一致
    if "vehicle_id" in parameters:
        vehicle_id = parameters["vehicle_id"]
        parameters["namesspace"] = f"drone_{vehicle_id}"
        print(f"已根据 vehicle_id 更新 namesspace 为：{parameters['namesspace']}")

def interactive_update(data):
    """
    基于用户输入交互式更新 YAML 数据
    :param data: YAML 文件数据
    """
    ros_parameters = data.get("/**", {}).get("ros__parameters", {})
    if not ros_parameters:
        print("未找到 'ros__parameters' 字段，无法进行交互更新！")
        return

    # 更新 parameters 部分
    parameters = ros_parameters.get("parameters", {})
    if parameters:
        update_parameters(parameters)
    else:
        print("未找到 'parameters' 字段，跳过参数更新。")

def main():
    # 定义 YAML 文件路径
    yaml_file_path = "/home/zhao/project2/wuhan_project/ros.yaml"  # 修改为实际文件路径

    # 加载 YAML 文件
    yaml_data = load_yaml(yaml_file_path)
    if yaml_data is None:
        print("加载 YAML 文件失败，程序退出！")
        return

    # 交互式更新 YAML 数据
    interactive_update(yaml_data)

    # 保存更新后的 YAML 文件
    save_yaml(yaml_file_path, yaml_data)

if __name__ == "__main__":
    main()
